!--------------------------------------------------------------------------------------------------!
!   CP2K: A general program to perform molecular dynamics simulations                              !
!   Copyright 2000-2025 CP2K developers group <https://cp2k.org>                                   !
!                                                                                                  !
!   SPDX-License-Identifier: GPL-2.0-or-later                                                      !
!--------------------------------------------------------------------------------------------------!

! **************************************************************************************************
!> \brief Routines for Geometry optimization using BFGS algorithm
!> \par History
!>      Module modified by Pierre-André Cazade [pcazade] 01.2020 - University of Limerick.
!>      Modifications enable Space Group Symmetry.
! **************************************************************************************************
MODULE bfgs_optimizer

   USE atomic_kind_list_types, ONLY: atomic_kind_list_type
   USE atomic_kind_types, ONLY: get_atomic_kind, &
                                get_atomic_kind_set
   USE cell_types, ONLY: cell_type, &
                         pbc
   USE constraint_fxd, ONLY: fix_atom_control
   USE cp_blacs_env, ONLY: cp_blacs_env_create, &
                           cp_blacs_env_release, &
                           cp_blacs_env_type
   USE cp_external_control, ONLY: external_control
   USE cp_files, ONLY: close_file, &
                       open_file
   USE cp_fm_basic_linalg, ONLY: cp_fm_column_scale, &
                                 cp_fm_transpose
   USE cp_fm_diag, ONLY: choose_eigv_solver
   USE cp_fm_struct, ONLY: cp_fm_struct_create, &
                           cp_fm_struct_release, &
                           cp_fm_struct_type
   USE cp_fm_types, ONLY: &
      cp_fm_get_info, &
      cp_fm_read_unformatted, &
      cp_fm_set_all, &
      cp_fm_to_fm, &
      cp_fm_type, &
      cp_fm_write_unformatted, cp_fm_create, cp_fm_release
   USE parallel_gemm_api, ONLY: parallel_gemm
   USE cp_log_handling, ONLY: cp_get_default_logger, &
                              cp_logger_type, &
                              cp_to_string
   USE cp_output_handling, ONLY: cp_iterate, &
                                 cp_p_file, &
                                 cp_print_key_finished_output, &
                                 cp_print_key_should_output, &
                                 cp_print_key_unit_nr
   USE message_passing, ONLY: mp_para_env_type
   USE cp_subsys_types, ONLY: cp_subsys_get, &
                              cp_subsys_type
   USE force_env_types, ONLY: force_env_get, &
                              force_env_type
   USE global_types, ONLY: global_environment_type
   USE gopt_f_methods, ONLY: gopt_f_ii, &
                             gopt_f_io, &
                             gopt_f_io_finalize, &
                             gopt_f_io_init, &
                             print_geo_opt_header, &
                             print_geo_opt_nc
   USE gopt_f_types, ONLY: gopt_f_type
   USE gopt_param_types, ONLY: gopt_param_type
   USE input_constants, ONLY: default_cell_method_id, &
                              default_ts_method_id
   USE input_section_types, ONLY: section_vals_get_subs_vals, &
                                  section_vals_type, &
                                  section_vals_val_get, &
                                  section_vals_val_set
   USE kinds, ONLY: default_path_length, &
                    dp
   USE machine, ONLY: m_flush, &
                      m_walltime
   USE particle_list_types, ONLY: particle_list_type
   USE space_groups, ONLY: identify_space_group, &
                           print_spgr, &
                           spgr_apply_rotations_coord, &
                           spgr_apply_rotations_force
   USE space_groups_types, ONLY: spgr_type
#include "../base/base_uses.f90"

   IMPLICIT NONE
   PRIVATE

   #:include "gopt_f77_methods.fypp"

   CHARACTER(len=*), PARAMETER, PRIVATE :: moduleN = 'bfgs_optimizer'
   LOGICAL, PARAMETER                   :: debug_this_module = .TRUE.

   PUBLIC :: geoopt_bfgs

CONTAINS

! **************************************************************************************************
!> \brief Main driver for BFGS geometry optimizations
!> \param force_env ...
!> \param gopt_param ...
!> \param globenv ...
!> \param geo_section ...
!> \param gopt_env ...
!> \param x0 ...
!> \par History
!>      01.2020 modified to perform Space Group Symmetry [pcazade]
! **************************************************************************************************
   RECURSIVE SUBROUTINE geoopt_bfgs(force_env, gopt_param, globenv, geo_section, gopt_env, x0)

      TYPE(force_env_type), POINTER                      :: force_env
      TYPE(gopt_param_type), POINTER                     :: gopt_param
      TYPE(global_environment_type), POINTER             :: globenv
      TYPE(section_vals_type), POINTER                   :: geo_section
      TYPE(gopt_f_type), POINTER                         :: gopt_env
      REAL(KIND=dp), DIMENSION(:), POINTER               :: x0

      CHARACTER(len=*), PARAMETER                        :: routineN = 'geoopt_bfgs'
      REAL(KIND=dp), PARAMETER                           :: one = 1.0_dp, zero = 0.0_dp

      CHARACTER(LEN=5)                                   :: wildcard
      CHARACTER(LEN=default_path_length)                 :: hes_filename
      INTEGER                                            :: handle, hesunit_read, indf, info, &
                                                            iter_nr, its, maxiter, ndf, nfree, &
                                                            output_unit
      LOGICAL                                            :: conv, hesrest, ionode, shell_present, &
                                                            should_stop, use_mod_hes, use_rfo
      REAL(KIND=dp)                                      :: ediff, emin, eold, etot, pred, rad, rat, &
                                                            step, t_diff, t_now, t_old
      REAL(KIND=dp), ALLOCATABLE, DIMENSION(:)           :: dg, dr, dx, eigval, gold, work, xold
      REAL(KIND=dp), DIMENSION(:), POINTER               :: g
      TYPE(atomic_kind_list_type), POINTER               :: atomic_kinds
      TYPE(cp_blacs_env_type), POINTER                   :: blacs_env
      TYPE(cp_fm_struct_type), POINTER                   :: fm_struct_hes
      TYPE(cp_fm_type)                          :: eigvec_mat, hess_mat, hess_tmp
      TYPE(cp_logger_type), POINTER                      :: logger
      TYPE(mp_para_env_type), POINTER                    :: para_env
      TYPE(cp_subsys_type), POINTER                      :: subsys
      TYPE(section_vals_type), POINTER                   :: print_key, root_section
      TYPE(spgr_type), POINTER                           :: spgr

      NULLIFY (logger, g, blacs_env, spgr)
      logger => cp_get_default_logger()
      para_env => force_env%para_env
      root_section => force_env%root_section
      spgr => gopt_env%spgr
      t_old = m_walltime()

      CALL timeset(routineN, handle)
      CALL section_vals_val_get(geo_section, "BFGS%TRUST_RADIUS", r_val=rad)
      print_key => section_vals_get_subs_vals(geo_section, "BFGS%RESTART")
      ionode = para_env%is_source()
      maxiter = gopt_param%max_iter
      conv = .FALSE.
      rat = 0.0_dp
      wildcard = " BFGS"
      hes_filename = ""

      ! Stop if not yet implemented
      SELECT CASE (gopt_env%type_id)
      CASE (default_ts_method_id)
         CPABORT("BFGS method not yet working with DIMER")
      END SELECT

      CALL section_vals_val_get(geo_section, "BFGS%USE_RAT_FUN_OPT", l_val=use_rfo)
      CALL section_vals_val_get(geo_section, "BFGS%USE_MODEL_HESSIAN", l_val=use_mod_hes)
      CALL section_vals_val_get(geo_section, "BFGS%RESTART_HESSIAN", l_val=hesrest)
      output_unit = cp_print_key_unit_nr(logger, geo_section, "PRINT%PROGRAM_RUN_INFO", &
                                         extension=".geoLog")
      IF (output_unit > 0) THEN
         IF (use_rfo) THEN
            WRITE (UNIT=output_unit, FMT="(/,T2,A,T78,A3)") &
               "BFGS| Use rational function optimization for step estimation: ", "YES"
         ELSE
            WRITE (UNIT=output_unit, FMT="(/,T2,A,T78,A3)") &
               "BFGS| Use rational function optimization for step estimation: ", " NO"
         END IF
         IF (use_mod_hes) THEN
            WRITE (UNIT=output_unit, FMT="(T2,A,T78,A3)") &
               "BFGS| Use model Hessian for initial guess: ", "YES"
         ELSE
            WRITE (UNIT=output_unit, FMT="(T2,A,T78,A3)") &
               "BFGS| Use model Hessian for initial guess: ", " NO"
         END IF
         IF (hesrest) THEN
            WRITE (UNIT=output_unit, FMT="(T2,A,T78,A3)") &
               "BFGS| Restart Hessian: ", "YES"
         ELSE
            WRITE (UNIT=output_unit, FMT="(T2,A,T78,A3)") &
               "BFGS| Restart Hessian: ", " NO"
         END IF
         WRITE (UNIT=output_unit, FMT="(T2,A,T61,F20.3)") &
            "BFGS| Trust radius: ", rad
      END IF

      ndf = SIZE(x0)
      nfree = gopt_env%nfree
      IF (ndf > 3000) &
         CALL cp_warn(__LOCATION__, &
                      "The dimension of the Hessian matrix ("// &
                      TRIM(ADJUSTL(cp_to_string(ndf)))//") is greater than 3000. "// &
                      "The diagonalisation of the full Hessian  matrix needed for BFGS "// &
                      "is computationally expensive. You should consider to use the linear "// &
                      "scaling variant L-BFGS instead.")

      ! Initialize hessian (hes = unitary matrix or model hessian )
      CALL cp_blacs_env_create(blacs_env, para_env, globenv%blacs_grid_layout, &
                               globenv%blacs_repeatable)
      CALL cp_fm_struct_create(fm_struct_hes, para_env=para_env, context=blacs_env, &
                               nrow_global=ndf, ncol_global=ndf)
      CALL cp_fm_create(hess_mat, fm_struct_hes, name="hess_mat")
      CALL cp_fm_create(hess_tmp, fm_struct_hes, name="hess_tmp")
      CALL cp_fm_create(eigvec_mat, fm_struct_hes, name="eigvec_mat")
      ALLOCATE (eigval(ndf))
      eigval(:) = 0.0_dp

      CALL force_env_get(force_env=force_env, subsys=subsys)
      CALL cp_subsys_get(subsys, atomic_kinds=atomic_kinds)
      CALL get_atomic_kind_set(atomic_kind_set=atomic_kinds%els, shell_present=shell_present)
      IF (use_mod_hes) THEN
         IF (shell_present) THEN
            CALL cp_warn(__LOCATION__, &
                         "No model Hessian is available for core-shell models. "// &
                         "A unit matrix is used as the initial Hessian.")
            use_mod_hes = .FALSE.
         END IF
         IF (gopt_env%type_id == default_cell_method_id) THEN
            CALL cp_warn(__LOCATION__, &
                         "No model Hessian is available for cell optimizations. "// &
                         "A unit matrix is used as the initial Hessian.")
            use_mod_hes = .FALSE.
         END IF
      END IF

      IF (use_mod_hes) THEN
         CALL cp_fm_set_all(hess_mat, alpha=zero, beta=0.00_dp)
         CALL construct_initial_hess(gopt_env%force_env, hess_mat)
         CALL cp_fm_to_fm(hess_mat, hess_tmp)
         CALL choose_eigv_solver(hess_tmp, eigvec_mat, eigval, info=info)
         ! In rare cases the diagonalization of hess_mat fails (bug in scalapack?)
         IF (info /= 0) THEN
            CALL cp_fm_set_all(hess_mat, alpha=zero, beta=one)
            IF (output_unit > 0) WRITE (output_unit, *) &
               "BFGS: Matrix diagonalization failed, using unity as model Hessian."
         ELSE
            DO its = 1, SIZE(eigval)
               IF (eigval(its) < 0.1_dp) eigval(its) = 0.1_dp
            END DO
            CALL cp_fm_to_fm(eigvec_mat, hess_tmp)
            CALL cp_fm_column_scale(eigvec_mat, eigval)
            CALL parallel_gemm("N", "T", ndf, ndf, ndf, one, hess_tmp, eigvec_mat, zero, hess_mat)
         END IF
      ELSE
         CALL cp_fm_set_all(hess_mat, alpha=zero, beta=one)
      END IF

      ALLOCATE (xold(ndf))
      xold(:) = x0(:)

      ALLOCATE (g(ndf))
      g(:) = 0.0_dp

      ALLOCATE (gold(ndf))
      gold(:) = 0.0_dp

      ALLOCATE (dx(ndf))
      dx(:) = 0.0_dp

      ALLOCATE (dg(ndf))
      dg(:) = 0.0_dp

      ALLOCATE (work(ndf))
      work(:) = 0.0_dp

      ALLOCATE (dr(ndf))
      dr(:) = 0.0_dp

      ! find space_group
      CALL section_vals_val_get(geo_section, "KEEP_SPACE_GROUP", l_val=spgr%keep_space_group)
      IF (spgr%keep_space_group) THEN
         CALL identify_space_group(subsys, geo_section, gopt_env, output_unit)
         CALL spgr_apply_rotations_coord(spgr, x0)
         CALL print_spgr(spgr)
      END IF

      ! Geometry optimization starts now
      CALL cp_iterate(logger%iter_info, increment=0, iter_nr_out=iter_nr)
      CALL print_geo_opt_header(gopt_env, output_unit, wildcard)

      ! Calculate Energy & Gradients
      CALL cp_eval_at(gopt_env, x0, etot, g, gopt_env%force_env%para_env%mepos, &
                      .FALSE., gopt_env%force_env%para_env)

      ! Symmetrize coordinates and forces
      IF (spgr%keep_space_group) THEN
         CALL spgr_apply_rotations_coord(spgr, x0)
         CALL spgr_apply_rotations_force(spgr, g)
      END IF

      ! Print info at time 0
      emin = etot
      t_now = m_walltime()
      t_diff = t_now - t_old
      t_old = t_now
      CALL gopt_f_io_init(gopt_env, output_unit, etot, wildcard=wildcard, its=iter_nr, used_time=t_diff)
      DO its = iter_nr + 1, maxiter
         CALL cp_iterate(logger%iter_info, last=(its == maxiter))
         CALL section_vals_val_set(geo_section, "STEP_START_VAL", i_val=its)
         CALL gopt_f_ii(its, output_unit)

         ! Hessian update/restarting
         IF (((its - iter_nr) == 1) .AND. hesrest) THEN
            IF (ionode) THEN
               CALL section_vals_val_get(geo_section, "BFGS%RESTART_FILE_NAME", c_val=hes_filename)
               IF (LEN_TRIM(hes_filename) == 0) THEN
                  ! Set default Hessian restart file name if no file name is defined
                  hes_filename = TRIM(logger%iter_info%project_name)//"-BFGS.Hessian"
               END IF
               IF (output_unit > 0) THEN
                  WRITE (UNIT=output_unit, FMT="(/,T2,A)") &
                     "BFGS| Checking for Hessian restart file <"//TRIM(ADJUSTL(hes_filename))//">"
               END IF
               CALL open_file(file_name=TRIM(hes_filename), file_status="OLD", &
                              file_form="UNFORMATTED", file_action="READ", unit_number=hesunit_read)
               IF (output_unit > 0) THEN
                  WRITE (UNIT=output_unit, FMT="(T2,A)") &
                     "BFGS| Hessian restart file read"
               END IF
            END IF
            CALL cp_fm_read_unformatted(hess_mat, hesunit_read)
            IF (ionode) CALL close_file(unit_number=hesunit_read)
         ELSE
            IF ((its - iter_nr) > 1) THEN
               ! Symmetrize old coordinates and old forces
               IF (spgr%keep_space_group) THEN
                  CALL spgr_apply_rotations_coord(spgr, xold)
                  CALL spgr_apply_rotations_force(spgr, gold)
               END IF

               DO indf = 1, ndf
                  dx(indf) = x0(indf) - xold(indf)
                  dg(indf) = g(indf) - gold(indf)
               END DO

               CALL bfgs(ndf, dx, dg, hess_mat, work, para_env)

               ! Symmetrize coordinates and forces change
               IF (spgr%keep_space_group) THEN
                  CALL spgr_apply_rotations_force(spgr, dx)
                  CALL spgr_apply_rotations_force(spgr, dg)
               END IF

               !Possibly dump the Hessian file
               IF (BTEST(cp_print_key_should_output(logger%iter_info, print_key), cp_p_file)) THEN
                  CALL write_bfgs_hessian(geo_section, hess_mat, logger)
               END IF
            END IF
         END IF

         ! Symmetrize coordinates and forces
         IF (spgr%keep_space_group) THEN
            CALL spgr_apply_rotations_coord(spgr, x0)
            CALL spgr_apply_rotations_force(spgr, g)
         END IF

         ! Setting the present positions & gradients as old
         xold(:) = x0
         gold(:) = g

         ! Copying hessian hes to (ndf x ndf) matrix hes_mat for diagonalization
         CALL cp_fm_to_fm(hess_mat, hess_tmp)

         CALL choose_eigv_solver(hess_tmp, eigvec_mat, eigval, info=info)

         ! In rare cases the diagonalization of hess_mat fails (bug in scalapack?)
         IF (info /= 0) THEN
            IF (output_unit > 0) WRITE (output_unit, *) &
               "BFGS: Matrix diagonalization failed, resetting Hessian to unity."
            CALL cp_fm_set_all(hess_mat, alpha=zero, beta=one)
            CALL cp_fm_to_fm(hess_mat, hess_tmp)
            CALL choose_eigv_solver(hess_tmp, eigvec_mat, eigval)
         END IF

         IF (use_rfo) THEN
            CALL set_hes_eig(ndf, eigval, work)
            dx(:) = eigval
            CALL rat_fun_opt(ndf, dg, eigval, work, eigvec_mat, g, para_env)
         END IF
         CALL geoopt_get_step(ndf, eigval, eigvec_mat, hess_tmp, dr, g, para_env, use_rfo)

         ! Symmetrize dr
         IF (spgr%keep_space_group) THEN
            CALL spgr_apply_rotations_force(spgr, dr)
         END IF

         CALL trust_radius(ndf, step, rad, rat, dr, output_unit)

         ! Update the atomic positions
         x0 = x0 + dr

         ! Symmetrize coordinates
         IF (spgr%keep_space_group) THEN
            CALL spgr_apply_rotations_coord(spgr, x0)
         END IF

         CALL energy_predict(ndf, work, hess_mat, dr, g, conv, pred, para_env)
         eold = etot

         ! Energy & Gradients at new step
         CALL cp_eval_at(gopt_env, x0, etot, g, gopt_env%force_env%para_env%mepos, &
                         .FALSE., gopt_env%force_env%para_env)

         ediff = etot - eold

         ! Symmetrize forces
         IF (spgr%keep_space_group) THEN
            CALL spgr_apply_rotations_force(spgr, g)
         END IF

         ! check for an external exit command
         CALL external_control(should_stop, "GEO", globenv=globenv)
         IF (should_stop) EXIT

         ! Some IO and Convergence check
         t_now = m_walltime()
         t_diff = t_now - t_old
         t_old = t_now
         CALL gopt_f_io(gopt_env, force_env, root_section, its, etot, output_unit, &
                        eold, emin, wildcard, gopt_param, ndf, dr, g, conv, pred, rat, &
                        step, rad, used_time=t_diff)

         IF (conv .OR. (its == maxiter)) EXIT
         IF (etot < emin) emin = etot
         IF (use_rfo) CALL update_trust_rad(rat, rad, step, ediff)
      END DO

      IF (its == maxiter .AND. (.NOT. conv)) THEN
         CALL print_geo_opt_nc(gopt_env, output_unit)
      END IF

      ! show space_group
      CALL section_vals_val_get(geo_section, "SHOW_SPACE_GROUP", l_val=spgr%show_space_group)
      IF (spgr%show_space_group) THEN
         CALL identify_space_group(subsys, geo_section, gopt_env, output_unit)
         CALL print_spgr(spgr)
      END IF

      ! Write final  information, if converged
      CALL cp_iterate(logger%iter_info, last=.TRUE., increment=0)
      CALL write_bfgs_hessian(geo_section, hess_mat, logger)
      CALL gopt_f_io_finalize(gopt_env, force_env, x0, conv, its, root_section, &
                              gopt_env%force_env%para_env, gopt_env%force_env%para_env%mepos, output_unit)

      CALL cp_fm_struct_release(fm_struct_hes)
      CALL cp_fm_release(hess_mat)
      CALL cp_fm_release(eigvec_mat)
      CALL cp_fm_release(hess_tmp)

      CALL cp_blacs_env_release(blacs_env)
      DEALLOCATE (xold)
      DEALLOCATE (g)
      DEALLOCATE (gold)
      DEALLOCATE (dx)
      DEALLOCATE (dg)
      DEALLOCATE (eigval)
      DEALLOCATE (work)
      DEALLOCATE (dr)

      CALL cp_print_key_finished_output(output_unit, logger, geo_section, &
                                        "PRINT%PROGRAM_RUN_INFO")
      CALL timestop(handle)

   END SUBROUTINE geoopt_bfgs

! **************************************************************************************************
!> \brief ...
!> \param ndf ...
!> \param dg ...
!> \param eigval ...
!> \param work ...
!> \param eigvec_mat ...
!> \param g ...
!> \param para_env ...
! **************************************************************************************************
   SUBROUTINE rat_fun_opt(ndf, dg, eigval, work, eigvec_mat, g, para_env)

      INTEGER, INTENT(IN)                                :: ndf
      REAL(KIND=dp), INTENT(INOUT)                       :: dg(ndf), eigval(ndf), work(ndf)
      TYPE(cp_fm_type), INTENT(IN)                       :: eigvec_mat
      REAL(KIND=dp), INTENT(INOUT)                       :: g(ndf)
      TYPE(mp_para_env_type), OPTIONAL, POINTER          :: para_env

      CHARACTER(LEN=*), PARAMETER                        :: routineN = 'rat_fun_opt'
      REAL(KIND=dp), PARAMETER                           :: one = 1.0_dp

      INTEGER                                            :: handle, i, indf, iref, iter, j, k, l, &
                                                            maxit, ncol_local, nrow_local
      INTEGER, DIMENSION(:), POINTER                     :: col_indices, row_indices
      LOGICAL                                            :: bisec, fail, set
      REAL(KIND=dp)                                      :: fun, fun1, fun2, fun3, fung, lam1, lam2, &
                                                            ln, lp, ssize, step, stol
      REAL(KIND=dp), CONTIGUOUS, DIMENSION(:, :), POINTER            :: local_data

      CALL timeset(routineN, handle)

      stol = 1.0E-8_dp
      ssize = 0.2_dp
      maxit = 999
      fail = .FALSE.
      bisec = .FALSE.

      dg = 0._dp

      CALL cp_fm_get_info(eigvec_mat, row_indices=row_indices, col_indices=col_indices, &
                          local_data=local_data, nrow_local=nrow_local, ncol_local=ncol_local)

      DO i = 1, nrow_local
         j = row_indices(i)
         DO k = 1, ncol_local
            l = col_indices(k)
            dg(l) = dg(l) + local_data(i, k)*g(j)
         END DO
      END DO
      CALL para_env%sum(dg)

      set = .FALSE.

      DO

!   calculating Lambda

         lp = 0.0_dp
         iref = 1
         ln = 0.0_dp
         IF (eigval(iref) < 0.0_dp) ln = eigval(iref) - 0.01_dp

         iter = 0
         DO
            iter = iter + 1
            fun = 0.0_dp
            fung = 0.0_dp
            DO indf = 1, ndf
               fun = fun + dg(indf)**2/(ln - eigval(indf))
               fung = fung - dg(indf)**2/(ln - eigval(indf)**2)
            END DO
            fun = fun - ln
            fung = fung - one
            step = fun/fung
            ln = ln - step
            IF (ABS(step) < stol) GOTO 200
            IF (iter >= maxit) EXIT
         END DO
100      CONTINUE
         bisec = .TRUE.
         iter = 0
         maxit = 9999
         lam1 = 0.0_dp
         IF (eigval(iref) < 0.0_dp) lam1 = eigval(iref) - 0.01_dp
         fun1 = 0.0_dp
         DO indf = 1, ndf
            fun1 = fun1 + dg(indf)**2/(lam1 - eigval(indf))
         END DO
         fun1 = fun1 - lam1
         step = ABS(lam1)/1000.0_dp
         IF (step < ssize) step = ssize
         DO
            iter = iter + 1
            IF (iter > maxit) THEN
               ln = 0.0_dp
               lp = 0.0_dp
               fail = .TRUE.
               GOTO 300
            END IF
            fun2 = 0.0_dp
            lam2 = lam1 - iter*step
            DO indf = 1, ndf
               fun2 = fun2 + eigval(indf)**2/(lam2 - eigval(indf))
            END DO
            fun2 = fun2 - lam2
            IF (fun2*fun1 < 0.0_dp) THEN
               iter = 0
               DO
                  iter = iter + 1
                  IF (iter > maxit) THEN
                     ln = 0.0_dp
                     lp = 0.0_dp
                     fail = .TRUE.
                     GOTO 300
                  END IF
                  step = (lam1 + lam2)/2
                  fun3 = 0.0_dp
                  DO indf = 1, ndf
                     fun3 = fun3 + dg(indf)**2/(step - eigval(indf))
                  END DO
                  fun3 = fun3 - step

                  IF (ABS(step - lam2) < stol) THEN
                     ln = step
                     GOTO 200
                  END IF

                  IF (fun3*fun1 < stol) THEN
                     lam2 = step
                  ELSE
                     lam1 = step
                  END IF
               END DO
            END IF
         END DO

200      CONTINUE
         IF ((ln > eigval(iref)) .OR. ((ln > 0.0_dp) .AND. &
                                       (eigval(iref) > 0.0_dp))) THEN

            IF (.NOT. bisec) GOTO 100
            ln = 0.0_dp
            lp = 0.0_dp
            fail = .TRUE.
         END IF

300      CONTINUE

         IF (fail .AND. .NOT. set) THEN
            set = .TRUE.
            DO indf = 1, ndf
               eigval(indf) = eigval(indf)*work(indf)
            END DO
            CYCLE
         END IF

         IF (.NOT. set) THEN
            work(1:ndf) = one
         END IF

         DO indf = 1, ndf
            eigval(indf) = eigval(indf) - ln
         END DO
         EXIT
      END DO

      CALL timestop(handle)

   END SUBROUTINE rat_fun_opt

! **************************************************************************************************
!> \brief ...
!> \param ndf ...
!> \param dx ...
!> \param dg ...
!> \param hess_mat ...
!> \param work ...
!> \param para_env ...
! **************************************************************************************************
   SUBROUTINE bfgs(ndf, dx, dg, hess_mat, work, para_env)
      INTEGER, INTENT(IN)                                :: ndf
      REAL(KIND=dp), INTENT(INOUT)                       :: dx(ndf), dg(ndf)
      TYPE(cp_fm_type), INTENT(IN)                       :: hess_mat
      REAL(KIND=dp), INTENT(INOUT)                       :: work(ndf)
      TYPE(mp_para_env_type), OPTIONAL, POINTER          :: para_env

      CHARACTER(LEN=*), PARAMETER                        :: routineN = 'bfgs'
      REAL(KIND=dp), PARAMETER                           :: one = 1.0_dp, zero = 0.0_dp

      INTEGER                                            :: handle, i, j, k, l, ncol_local, &
                                                            nrow_local
      INTEGER, DIMENSION(:), POINTER                     :: col_indices, row_indices
      REAL(KIND=dp)                                      :: DDOT, dxw, gdx
      REAL(KIND=dp), CONTIGUOUS, DIMENSION(:, :), POINTER            :: local_hes

      CALL timeset(routineN, handle)

      CALL cp_fm_get_info(hess_mat, row_indices=row_indices, col_indices=col_indices, &
                          local_data=local_hes, nrow_local=nrow_local, ncol_local=ncol_local)

      work = zero
      DO i = 1, nrow_local
         j = row_indices(i)
         DO k = 1, ncol_local
            l = col_indices(k)
            work(j) = work(j) + local_hes(i, k)*dx(l)
         END DO
      END DO

      CALL para_env%sum(work)

      gdx = DDOT(ndf, dg, 1, dx, 1)
      gdx = one/gdx
      dxw = DDOT(ndf, dx, 1, work, 1)
      dxw = one/dxw

      DO i = 1, nrow_local
         j = row_indices(i)
         DO k = 1, ncol_local
            l = col_indices(k)
            local_hes(i, k) = local_hes(i, k) + gdx*dg(j)*dg(l) - &
                              dxw*work(j)*work(l)
         END DO
      END DO

      CALL timestop(handle)

   END SUBROUTINE bfgs

! **************************************************************************************************
!> \brief ...
!> \param ndf ...
!> \param eigval ...
!> \param work ...
! **************************************************************************************************
   SUBROUTINE set_hes_eig(ndf, eigval, work)
      INTEGER, INTENT(IN)                                :: ndf
      REAL(KIND=dp), INTENT(INOUT)                       :: eigval(ndf), work(ndf)

      CHARACTER(LEN=*), PARAMETER                        :: routineN = 'set_hes_eig'
      REAL(KIND=dp), PARAMETER                           :: max_neg = -0.5_dp, max_pos = 5.0_dp, &
                                                            min_eig = 0.005_dp, one = 1.0_dp

      INTEGER                                            :: handle, indf
      LOGICAL                                            :: neg

      CALL timeset(routineN, handle)

      DO indf = 1, ndf
         IF (eigval(indf) < 0.0_dp) neg = .TRUE.
         IF (eigval(indf) > 1000.0_dp) eigval(indf) = 1000.0_dp
      END DO
      DO indf = 1, ndf
         IF (eigval(indf) < 0.0_dp) THEN
            IF (eigval(indf) < max_neg) THEN
               eigval(indf) = max_neg
            ELSE IF (eigval(indf) > -min_eig) THEN
               eigval(indf) = -min_eig
            END IF
         ELSE IF (eigval(indf) < 1000.0_dp) THEN
            IF (eigval(indf) < min_eig) THEN
               eigval(indf) = min_eig
            ELSE IF (eigval(indf) > max_pos) THEN
               eigval(indf) = max_pos
            END IF
         END IF
      END DO

      DO indf = 1, ndf
         IF (eigval(indf) < 0.0_dp) THEN
            work(indf) = -one
         ELSE
            work(indf) = one
         END IF
      END DO

      CALL timestop(handle)

   END SUBROUTINE set_hes_eig

! **************************************************************************************************
!> \brief ...
!> \param ndf ...
!> \param eigval ...
!> \param eigvec_mat ...
!> \param hess_tmp ...
!> \param dr ...
!> \param g ...
!> \param para_env ...
!> \param use_rfo ...
! **************************************************************************************************
   SUBROUTINE geoopt_get_step(ndf, eigval, eigvec_mat, hess_tmp, dr, g, para_env, use_rfo)

      INTEGER, INTENT(IN)                                :: ndf
      REAL(KIND=dp), INTENT(INOUT)                       :: eigval(ndf)
      TYPE(cp_fm_type), INTENT(IN)                       :: eigvec_mat, hess_tmp
      REAL(KIND=dp), INTENT(INOUT)                       :: dr(ndf), g(ndf)
      TYPE(mp_para_env_type), OPTIONAL, POINTER          :: para_env
      LOGICAL                                            :: use_rfo

      REAL(KIND=dp), PARAMETER                           :: one = 1.0_dp, zero = 0.0_dp

      INTEGER                                            :: i, indf, j, k, l, ncol_local, nrow_local
      INTEGER, DIMENSION(:), POINTER                     :: col_indices, row_indices
      REAL(KIND=dp), CONTIGUOUS, DIMENSION(:, :), POINTER            :: local_data
      TYPE(cp_fm_struct_type), POINTER                   :: matrix_struct
      TYPE(cp_fm_type)                          :: tmp

      CALL cp_fm_to_fm(eigvec_mat, hess_tmp)
      IF (use_rfo) THEN
         DO indf = 1, ndf
            eigval(indf) = one/eigval(indf)
         END DO
      ELSE
         DO indf = 1, ndf
            eigval(indf) = one/MAX(0.0001_dp, eigval(indf))
         END DO
      END IF

      CALL cp_fm_column_scale(hess_tmp, eigval)
      CALL cp_fm_get_info(eigvec_mat, matrix_struct=matrix_struct)
      CALL cp_fm_create(tmp, matrix_struct, name="tmp")

      CALL parallel_gemm("N", "T", ndf, ndf, ndf, one, hess_tmp, eigvec_mat, zero, tmp)

      CALL cp_fm_transpose(tmp, hess_tmp)
      CALL cp_fm_release(tmp)

!    ** New step **

      CALL cp_fm_get_info(hess_tmp, row_indices=row_indices, col_indices=col_indices, &
                          local_data=local_data, nrow_local=nrow_local, ncol_local=ncol_local)

      dr = 0.0_dp
      DO i = 1, nrow_local
         j = row_indices(i)
         DO k = 1, ncol_local
            l = col_indices(k)
            dr(j) = dr(j) - local_data(i, k)*g(l)
         END DO
      END DO

      CALL para_env%sum(dr)

   END SUBROUTINE geoopt_get_step

! **************************************************************************************************
!> \brief ...
!> \param ndf ...
!> \param step ...
!> \param rad ...
!> \param rat ...
!> \param dr ...
!> \param output_unit ...
! **************************************************************************************************
   SUBROUTINE trust_radius(ndf, step, rad, rat, dr, output_unit)
      INTEGER, INTENT(IN)                                :: ndf
      REAL(KIND=dp), INTENT(INOUT)                       :: step, rad, rat, dr(ndf)
      INTEGER, INTENT(IN)                                :: output_unit

      CHARACTER(LEN=*), PARAMETER                        :: routineN = 'trust_radius'
      REAL(KIND=dp), PARAMETER                           :: one = 1.0_dp

      INTEGER                                            :: handle
      REAL(KIND=dp)                                      :: scal

      CALL timeset(routineN, handle)

      step = MAXVAL(ABS(dr))
      scal = MAX(one, rad/step)

      IF (step > rad) THEN
         rat = rad/step
         CALL DSCAL(ndf, rat, dr, 1)
         step = rad
         IF (output_unit > 0) THEN
            WRITE (unit=output_unit, FMT="(/,T2,A,F8.5)") &
               " Step is scaled; Scaling factor = ", rat
            CALL m_flush(output_unit)
         END IF
      END IF
      CALL timestop(handle)

   END SUBROUTINE trust_radius

! **************************************************************************************************
!> \brief ...
!> \param ndf ...
!> \param work ...
!> \param hess_mat ...
!> \param dr ...
!> \param g ...
!> \param conv ...
!> \param pred ...
!> \param para_env ...
! **************************************************************************************************
   SUBROUTINE energy_predict(ndf, work, hess_mat, dr, g, conv, pred, para_env)

      INTEGER, INTENT(IN)                                :: ndf
      REAL(KIND=dp), INTENT(INOUT)                       :: work(ndf)
      TYPE(cp_fm_type), INTENT(IN)                           :: hess_mat
      REAL(KIND=dp), INTENT(INOUT)                       :: dr(ndf), g(ndf)
      LOGICAL, INTENT(INOUT)                             :: conv
      REAL(KIND=dp), INTENT(INOUT)                       :: pred
      TYPE(mp_para_env_type), POINTER                    :: para_env

      CHARACTER(LEN=*), PARAMETER                        :: routineN = 'energy_predict'
      REAL(KIND=dp), PARAMETER                           :: zero = 0.0_dp

      INTEGER                                            :: handle, i, j, k, l, ncol_local, &
                                                            nrow_local
      INTEGER, DIMENSION(:), POINTER                     :: col_indices, row_indices
      REAL(KIND=dp)                                      :: DDOT, ener1, ener2
      REAL(KIND=dp), CONTIGUOUS, DIMENSION(:, :), POINTER            :: local_data

      CALL timeset(routineN, handle)

      ener1 = DDOT(ndf, g, 1, dr, 1)

      CALL cp_fm_get_info(hess_mat, row_indices=row_indices, col_indices=col_indices, &
                          local_data=local_data, nrow_local=nrow_local, ncol_local=ncol_local)

      work = zero
      DO i = 1, nrow_local
         j = row_indices(i)
         DO k = 1, ncol_local
            l = col_indices(k)
            work(j) = work(j) + local_data(i, k)*dr(l)
         END DO
      END DO

      CALL para_env%sum(work)
      ener2 = DDOT(ndf, dr, 1, work, 1)
      pred = ener1 + 0.5_dp*ener2
      conv = .FALSE.
      CALL timestop(handle)

   END SUBROUTINE energy_predict

! **************************************************************************************************
!> \brief ...
!> \param rat ...
!> \param rad ...
!> \param step ...
!> \param ediff ...
! **************************************************************************************************
   SUBROUTINE update_trust_rad(rat, rad, step, ediff)

      REAL(KIND=dp), INTENT(INOUT)                       :: rat, rad, step, ediff

      CHARACTER(LEN=*), PARAMETER                        :: routineN = 'update_trust_rad'
      REAL(KIND=dp), PARAMETER                           :: max_trust = 1.0_dp, min_trust = 0.1_dp

      INTEGER                                            :: handle

      CALL timeset(routineN, handle)

      IF (rat > 4.0_dp) THEN
         IF (ediff < 0.0_dp) THEN
            rad = step*0.5_dp
         ELSE
            rad = step*0.25_dp
         END IF
      ELSE IF (rat > 2.0_dp) THEN
         IF (ediff < 0.0_dp) THEN
            rad = step*0.75_dp
         ELSE
            rad = step*0.5_dp
         END IF
      ELSE IF (rat > 4.0_dp/3.0_dp) THEN
         IF (ediff < 0.0_dp) THEN
            rad = step
         ELSE
            rad = step*0.75_dp
         END IF
      ELSE IF (rat > 10.0_dp/9.0_dp) THEN
         IF (ediff < 0.0_dp) THEN
            rad = step*1.25_dp
         ELSE
            rad = step
         END IF
      ELSE IF (rat > 0.9_dp) THEN
         IF (ediff < 0.0_dp) THEN
            rad = step*1.5_dp
         ELSE
            rad = step*1.25_dp
         END IF
      ELSE IF (rat > 0.75_dp) THEN
         IF (ediff < 0.0_dp) THEN
            rad = step*1.25_dp
         ELSE
            rad = step
         END IF
      ELSE IF (rat > 0.5_dp) THEN
         IF (ediff < 0.0_dp) THEN
            rad = step
         ELSE
            rad = step*0.75_dp
         END IF
      ELSE IF (rat > 0.25_dp) THEN
         IF (ediff < 0.0_dp) THEN
            rad = step*0.75_dp
         ELSE
            rad = step*0.5_dp
         END IF
      ELSE IF (ediff < 0.0_dp) THEN
         rad = step*0.5_dp
      ELSE
         rad = step*0.25_dp
      END IF

      rad = MAX(rad, min_trust)
      rad = MIN(rad, max_trust)
      CALL timestop(handle)

   END SUBROUTINE update_trust_rad

! **************************************************************************************************

! **************************************************************************************************
!> \brief ...
!> \param geo_section ...
!> \param hess_mat ...
!> \param logger ...
! **************************************************************************************************
   SUBROUTINE write_bfgs_hessian(geo_section, hess_mat, logger)

      TYPE(section_vals_type), POINTER                   :: geo_section
      TYPE(cp_fm_type), INTENT(IN)                          :: hess_mat
      TYPE(cp_logger_type), POINTER                      :: logger

      CHARACTER(LEN=*), PARAMETER :: routineN = 'write_bfgs_hessian'

      INTEGER                                            :: handle, hesunit

      CALL timeset(routineN, handle)

      hesunit = cp_print_key_unit_nr(logger, geo_section, "BFGS%RESTART", &
                                     extension=".Hessian", file_form="UNFORMATTED", file_action="WRITE", &
                                     file_position="REWIND")

      CALL cp_fm_write_unformatted(hess_mat, hesunit)

      CALL cp_print_key_finished_output(hesunit, logger, geo_section, "BFGS%RESTART")

      CALL timestop(handle)

   END SUBROUTINE write_bfgs_hessian
! **************************************************************************************************

! **************************************************************************************************
!> \brief ...
!> \param force_env ...
!> \param hess_mat ...
! **************************************************************************************************
   SUBROUTINE construct_initial_hess(force_env, hess_mat)

      TYPE(force_env_type), POINTER                      :: force_env
      TYPE(cp_fm_type), INTENT(IN)                          :: hess_mat

      INTEGER                                            :: i, iat_col, iat_row, iglobal, iind, j, &
                                                            jat_row, jglobal, jind, k, natom, &
                                                            ncol_local, nrow_local, z
      INTEGER, ALLOCATABLE, DIMENSION(:)                 :: at_row
      INTEGER, DIMENSION(:), POINTER                     :: col_indices, row_indices
      REAL(KIND=dp), ALLOCATABLE, DIMENSION(:, :)        :: d_ij, rho_ij
      REAL(KIND=dp), ALLOCATABLE, DIMENSION(:, :, :)     :: r_ij
      REAL(KIND=dp), DIMENSION(3, 3)                     :: alpha, r0
      REAL(KIND=dp), CONTIGUOUS, DIMENSION(:, :), POINTER            :: fixed, local_data
      TYPE(cell_type), POINTER                           :: cell
      TYPE(cp_subsys_type), POINTER                      :: subsys
      TYPE(particle_list_type), POINTER                  :: particles

      CALL force_env_get(force_env=force_env, subsys=subsys, cell=cell)
      CALL cp_subsys_get(subsys, &
                         particles=particles)

      alpha(1, :) = [1._dp, 0.3949_dp, 0.3949_dp]
      alpha(2, :) = [0.3494_dp, 0.2800_dp, 0.2800_dp]
      alpha(3, :) = [0.3494_dp, 0.2800_dp, 0.1800_dp]

      r0(1, :) = [1.35_dp, 2.10_dp, 2.53_dp]
      r0(2, :) = [2.10_dp, 2.87_dp, 3.40_dp]
      r0(3, :) = [2.53_dp, 3.40_dp, 3.40_dp]

      CALL cp_fm_get_info(hess_mat, row_indices=row_indices, col_indices=col_indices, &
                          local_data=local_data, nrow_local=nrow_local, ncol_local=ncol_local)
      natom = particles%n_els
      ALLOCATE (at_row(natom))
      ALLOCATE (rho_ij(natom, natom))
      ALLOCATE (d_ij(natom, natom))
      ALLOCATE (r_ij(natom, natom, 3))
      ALLOCATE (fixed(3, natom))
      fixed = 1.0_dp
      CALL fix_atom_control(force_env, fixed)
      DO i = 1, 3
         CALL hess_mat%matrix_struct%para_env%min(fixed(i, :))
      END DO
      rho_ij = 0
      !XXXX insert proper rows !XXX
      at_row = 3
      DO i = 1, natom
         CALL get_atomic_kind(atomic_kind=particles%els(i)%atomic_kind, z=z)
         IF (z <= 10) at_row(i) = 2
         IF (z <= 2) at_row(i) = 1
      END DO
      DO i = 2, natom
         iat_row = at_row(i)
         DO j = 1, i - 1
            jat_row = at_row(j)
            !pbc for a distance vector
            r_ij(j, i, :) = pbc(particles%els(i)%r, particles%els(j)%r, cell)
            r_ij(i, j, :) = -r_ij(j, i, :)
            d_ij(j, i) = SQRT(DOT_PRODUCT(r_ij(j, i, :), r_ij(j, i, :)))
            d_ij(i, j) = d_ij(j, i)
            rho_ij(j, i) = EXP(alpha(jat_row, iat_row)*(r0(jat_row, iat_row)**2 - d_ij(j, i)**2))
            rho_ij(i, j) = rho_ij(j, i)
         END DO
      END DO
      DO i = 1, ncol_local
         iglobal = col_indices(i)
         iind = MOD(iglobal - 1, 3) + 1
         iat_col = (iglobal + 2)/3
         IF (iat_col > natom) CYCLE
         DO j = 1, nrow_local
            jglobal = row_indices(j)
            jind = MOD(jglobal - 1, 3) + 1
            iat_row = (jglobal + 2)/3
            IF (iat_row > natom) CYCLE
            IF (iat_row /= iat_col) THEN
               IF (d_ij(iat_row, iat_col) < 6.0_dp) &
                  local_data(j, i) = local_data(j, i) + &
                                     angle_second_deriv(r_ij, d_ij, rho_ij, iind, jind, iat_col, iat_row, natom)
            ELSE
               local_data(j, i) = local_data(j, i) + &
                                  angle_second_deriv(r_ij, d_ij, rho_ij, iind, jind, iat_col, iat_row, natom)
            END IF
            IF (iat_col /= iat_row) THEN
               IF (d_ij(iat_row, iat_col) < 6.0_dp) &
                  local_data(j, i) = local_data(j, i) - &
                                     dist_second_deriv(r_ij(iat_col, iat_row, :), &
                                                       iind, jind, d_ij(iat_row, iat_col), rho_ij(iat_row, iat_col))
            ELSE
               DO k = 1, natom
                  IF (k == iat_col) CYCLE
                  IF (d_ij(iat_row, k) < 6.0_dp) &
                     local_data(j, i) = local_data(j, i) + &
                                        dist_second_deriv(r_ij(iat_col, k, :), &
                                                          iind, jind, d_ij(iat_row, k), rho_ij(iat_row, k))
               END DO
            END IF
            IF (fixed(jind, iat_row) < 0.5_dp .OR. fixed(iind, iat_col) < 0.5_dp) THEN
               local_data(j, i) = 0.0_dp
               IF (jind == iind .AND. iat_row == iat_col) local_data(j, i) = 1.0_dp
            END IF
         END DO
      END DO
      DEALLOCATE (fixed)
      DEALLOCATE (rho_ij)
      DEALLOCATE (d_ij)
      DEALLOCATE (r_ij)
      DEALLOCATE (at_row)

   END SUBROUTINE construct_initial_hess

! **************************************************************************************************
!> \brief ...
!> \param r1 ...
!> \param i ...
!> \param j ...
!> \param d ...
!> \param rho ...
!> \return ...
! **************************************************************************************************
   FUNCTION dist_second_deriv(r1, i, j, d, rho) RESULT(deriv)
      REAL(KIND=dp), DIMENSION(3)                        :: r1
      INTEGER                                            :: i, j
      REAL(KIND=dp)                                      :: d, rho, deriv

      deriv = 0.45_dp*rho*(r1(i)*r1(j))/d**2
   END FUNCTION dist_second_deriv

! **************************************************************************************************
!> \brief ...
!> \param r_ij ...
!> \param d_ij ...
!> \param rho_ij ...
!> \param idir ...
!> \param jdir ...
!> \param iat_der ...
!> \param jat_der ...
!> \param natom ...
!> \return ...
! **************************************************************************************************
   FUNCTION angle_second_deriv(r_ij, d_ij, rho_ij, idir, jdir, iat_der, jat_der, natom) RESULT(deriv)
      REAL(KIND=dp), DIMENSION(:, :, :)                  :: r_ij
      REAL(KIND=dp), DIMENSION(:, :)                     :: d_ij, rho_ij
      INTEGER                                            :: idir, jdir, iat_der, jat_der, natom
      REAL(KIND=dp)                                      :: deriv

      INTEGER                                            :: i, iat, idr, j, jat, jdr
      REAL(KIND=dp)                                      :: d12, d23, d31, D_mat(3, 2), denom1, &
                                                            denom2, denom3, ka1, ka2, ka3, rho12, &
                                                            rho23, rho31, rsst1, rsst2, rsst3
      REAL(KIND=dp), DIMENSION(3)                        :: r12, r23, r31

      deriv = 0._dp
      IF (iat_der == jat_der) THEN
         DO i = 1, natom - 1
            IF (rho_ij(iat_der, i) < 0.00001) CYCLE
            DO j = i + 1, natom
               IF (rho_ij(iat_der, j) < 0.00001) CYCLE
               IF (i == iat_der .OR. j == iat_der) CYCLE
               IF (iat_der < i .OR. iat_der > j) THEN
                  r12 = r_ij(iat_der, i, :); r23 = r_ij(i, j, :); r31 = r_ij(j, iat_der, :)
                  d12 = d_ij(iat_der, i); d23 = d_ij(i, j); d31 = d_ij(j, iat_der)
                  rho12 = rho_ij(iat_der, i); rho23 = rho_ij(i, j); rho31 = rho_ij(j, iat_der)
               ELSE
                  r12 = r_ij(iat_der, j, :); r23 = r_ij(j, i, :); r31 = r_ij(i, iat_der, :)
                  d12 = d_ij(iat_der, j); d23 = d_ij(j, i); d31 = d_ij(i, iat_der)
                  rho12 = rho_ij(iat_der, j); rho23 = rho_ij(j, i); rho31 = rho_ij(i, iat_der)
               END IF
               ka1 = 0.15_dp*rho12*rho23; ka2 = 0.15_dp*rho23*rho31; ka3 = 0.15_dp*rho31*rho12
               rsst1 = DOT_PRODUCT(r12, r23); rsst2 = DOT_PRODUCT(r23, r31); rsst3 = DOT_PRODUCT(r31, r12)
               denom1 = 1.0_dp - rsst1**2/(d12**2*d23**2); denom2 = 1.0_dp - rsst2**2/(d23**2*d31**2)
               denom3 = 1.0_dp - rsst3**2/(d31**2*d12**2)
               denom1 = SIGN(1.0_dp, denom1)*MAX(ABS(denom1), 0.01_dp)
               denom2 = SIGN(1.0_dp, denom2)*MAX(ABS(denom2), 0.01_dp)
               denom3 = SIGN(1.0_dp, denom3)*MAX(ABS(denom3), 0.01_dp)
               D_mat(1, 1) = r23(idir)/(d12*d23) - rsst1*r12(idir)/(d12**3*d23)
               D_mat(1, 2) = r23(jdir)/(d12*d23) - rsst1*r12(jdir)/(d12**3*d23)
               D_mat(2, 1) = -r23(idir)/(d23*d31) + rsst2*r31(idir)/(d23*d31**3)
               D_mat(2, 2) = -r23(jdir)/(d23*d31) + rsst2*r31(jdir)/(d23*d31**3)
               D_mat(3, 1) = (r31(idir) - r12(idir))/(d31*d12) + rsst3*r31(idir)/(d31**3*d12) - &
                             rsst3*r12(idir)/(d31*d12**3)
               D_mat(3, 2) = (r31(jdir) - r12(jdir))/(d31*d12) + rsst3*r31(jdir)/(d31**3*d12) - &
                             rsst3*r12(jdir)/(d31*d12**3)
               IF (ABS(denom1) <= 0.011_dp) D_mat(1, 1) = 0.0_dp
               IF (ABS(denom2) <= 0.011_dp) D_mat(2, 1) = 0.0_dp
               IF (ABS(denom3) <= 0.011_dp) D_mat(3, 1) = 0.0_dp
               deriv = deriv + ka1*D_mat(1, 1)*D_mat(1, 2)/denom1 + &
                       ka2*D_mat(2, 1)*D_mat(2, 2)/denom2 + &
                       ka3*D_mat(3, 1)*D_mat(3, 2)/denom3

            END DO
         END DO
      ELSE
         DO i = 1, natom
            IF (i == iat_der .OR. i == jat_der) CYCLE
            IF (jat_der < iat_der) THEN
               iat = jat_der; jat = iat_der; idr = jdir; jdr = idir
            ELSE
               iat = iat_der; jat = jat_der; idr = idir; jdr = jdir
            END IF
            IF (jat < i .OR. iat > i) THEN
               r12 = r_ij(iat, jat, :); r23 = r_ij(jat, i, :); r31 = r_ij(i, iat, :)
               d12 = d_ij(iat, jat); d23 = d_ij(jat, i); d31 = d_ij(i, iat)
               rho12 = rho_ij(iat, jat); rho23 = rho_ij(jat, i); rho31 = rho_ij(i, iat)
            ELSE
               r12 = r_ij(iat, i, :); r23 = r_ij(i, jat, :); r31 = r_ij(jat, iat, :)
               d12 = d_ij(iat, i); d23 = d_ij(i, jat); d31 = d_ij(jat, iat)
               rho12 = rho_ij(iat, i); rho23 = rho_ij(i, jat); rho31 = rho_ij(jat, iat)
            END IF
            ka1 = 0.15_dp*rho12*rho23; ka2 = 0.15_dp*rho23*rho31; ka3 = 0.15_dp*rho31*rho12
            rsst1 = DOT_PRODUCT(r12, r23); rsst2 = DOT_PRODUCT(r23, r31); rsst3 = DOT_PRODUCT(r31, r12)
            denom1 = 1.0_dp - rsst1**2/(d12**2*d23**2); denom2 = 1.0_dp - rsst2**2/(d23**2*d31**2)
            denom3 = 1.0_dp - rsst3**2/(d31**2*d12**2)
            denom1 = SIGN(1.0_dp, denom1)*MAX(ABS(denom1), 0.01_dp)
            denom2 = SIGN(1.0_dp, denom2)*MAX(ABS(denom2), 0.01_dp)
            denom3 = SIGN(1.0_dp, denom3)*MAX(ABS(denom3), 0.01_dp)
            D_mat(1, 1) = r23(idr)/(d12*d23) - rsst1*r12(idr)/(d12**3*d23)
            D_mat(2, 1) = -r23(idr)/(d23*d31) + rsst2*r31(idr)/(d23*d31**3)
            D_mat(3, 1) = (r31(idr) - r12(idr))/(d31*d12) + rsst3*r31(idr)/(d31**3*d12) - &
                          rsst3*r12(idr)/(d31*d12**3)
            IF (jat < i .OR. iat > i) THEN
               D_mat(1, 2) = (r12(jdr) - r23(jdr))/(d12*d23) + rsst1*r12(jdr)/(d12**3*d23) - &
                             rsst1*r23(jdr)/(d12*d23**3)
               D_mat(2, 2) = r31(jdr)/(d23*d31) - rsst2*r23(jdr)/(d23**3*d31)
               D_mat(3, 2) = -r31(jdr)/(d31*d12) + rsst3*r12(jdr)/(d31*d12**3)
            ELSE
               D_mat(1, 2) = -r12(jdr)/(d12*d23) + rsst1*r23(jdr)/(d12*d23**3)
               D_mat(2, 2) = (r23(jdr) - r31(jdr))/(d23*d31) + rsst2*r23(jdr)/(d23**3*d31) - &
                             rsst2*r31(jdr)/(d23*d31**3)
               D_mat(3, 2) = r12(jdr)/(d31*d12) - rsst3*r31(jdr)/(d31**3*d12)
            END IF
            IF (ABS(denom1) <= 0.011_dp) D_mat(1, 1) = 0.0_dp
            IF (ABS(denom2) <= 0.011_dp) D_mat(2, 1) = 0.0_dp
            IF (ABS(denom3) <= 0.011_dp) D_mat(3, 1) = 0.0_dp

            deriv = deriv + ka1*D_mat(1, 1)*D_mat(1, 2)/denom1 + &
                    ka2*D_mat(2, 1)*D_mat(2, 2)/denom2 + &
                    ka3*D_mat(3, 1)*D_mat(3, 2)/denom3
         END DO
      END IF
      deriv = 0.25_dp*deriv

   END FUNCTION angle_second_deriv

END MODULE bfgs_optimizer
